/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.PIDSource;

/**
 *
 * @author Mark
 */
public class REncoder implements PIDSource
{
    AnalogChannel raw;
    double angleOffset = 0.0;
    static int maxRaw = 975;
    int preRaw = 0;
    int curRaw = 0;
    int numTurns = 0;
    double curAngle = 0.0;
    String name;
    boolean start = true;

    public REncoder(int slot, int channel, String a_name)
    {
        raw = new AnalogChannel(slot, channel);

        name = a_name;
    }

    /**
     * Method used by PIDController
     */
    public double pidGet()
    {
        return calculateAngle();
    }


    public double getAngle()
    {
        return calculateAngle();
    }


    public int getRawValue()
    {
        return raw.getValue();
    }


    public void zeroAngle()
    {
        angleOffset = 0.0;
        numTurns = 0;
        angleOffset = calculateAngle();

        System.out.println(name + " after zero has angle: " + calculateAngle() + " NumTurns: " + numTurns);

    }
    

    /**
     * Scales the raw value of the analog input into degrees, in the range
     * [0,360)
     */
    private double calculateAngle()
    {
        int aiRaw = raw.getValue();     // analog input

        // limit raw encoder values
        curRaw = aiRaw;
        if( curRaw < 0 )
            curRaw = 0;
        if( curRaw > maxRaw )
            curRaw = maxRaw;

        // invert encoder direction (because of mechanical mounting)
        curRaw = maxRaw - curRaw;
        
        if(start)
        {
            preRaw = curRaw;
            start = false;
        }

        // count encoder turns
        if( preRaw > maxRaw-150 && curRaw < 150 )
        {
            ++numTurns;
       //     System.out.println(name + " PreRaw: " + preRaw + " CurRaw: " + curRaw);
            preRaw = curRaw;
        //    System.out.println(name + "NumTurn: " + numTurns + "   --------------------------------- (+)");
        }
        else if( preRaw < 150 && curRaw > maxRaw-150 )
        {
            --numTurns;
            preRaw = curRaw;
            //System.out.println(name + "NumTurn: " + numTurns + "   --------------------------------- (-)");
        }
        else if( Math.abs(curRaw - preRaw) < 150 )
            preRaw = curRaw;                            // only remember if not something weird
        //else if( Math.abs(curRaw - preRaw) >= 150 )
            //System.out.println(name + "NumTurn: " + numTurns + "   --------------------------------- (?)");




        // convert to steering angle (in degrees)
        // there is a 10:32 reduction from encoder to actual wheel
        double angle = ( (double)numTurns + (double)curRaw/(double)maxRaw )*112.5 - angleOffset;

        // keep the angle in the range [0,360)
        angle = angle%360.0;
        if (angle < 0)
                angle += 360;

        // update the class variable
        curAngle = angle;

        // print it out
        //String s = "";
        //s = s + Utils.StringFormat6d(aiRaw);
        //s = s + "    ";
        //s = s + Utils.StringFormat6d(curRaw);
        //s = s + "    ";
        //s = s + Utils.StringFormat6d(numTurns);
        //s = s + "    ";
        //s = s + Utils.StringFormat61f(angle);

        //System.out.println(s);

        
        // return
        return angle;
    }

}
